## Untitled - By: STM32 - 周四 四月 19 2018
import sensor,image,time,lcd
from pyb import UART,Timer,LED

lcd.init()



rgb_threshold = {'r':[ 10, 84, 25, 119, 4, 112],
                'g':[10, 84, -110, -42, -10, 70],
                'b':[10, 83, -20, 69, -112, -4]}



def Dis_Circle(b,img):
    #计算矩形面积
    recA = b[2] * b[3]

    recY = (b[5]-b[0])*(b[5]-b[0])*3.14
    #计算像素和内接圆的比值
    y_B = recY/b[4]
    #计算像素值和矩形的比值
    y_A = recA/b[4]
    #外接圆
    y_C = (b[2]*b[2]/4+b[3]*b[3]/4)*3.14/b[4]
    #判断是不是圆型
    #print(y_B," ",y_A)
    if y_A < 1.3 and y_A > 1:
          if  y_B <1.5 and y_B >0.7:
                if(abs(b[2]-b[3]) <= 4):
                    img.draw_string(b[5],b[6], "C")
                    return 1
                 #print("C",b[2], b[3])
          else:
             img.draw_string(b[5],b[6], "R ")
             #print("R",b[2], b[3])
    return 0
     ##判断是不是矩形
    #el

    ##判断是不是菱形
    #if y_A >0.9 and y_A < 1.1:
        #if y_B > 0.9 and y_B < 1.1:
         #img.draw_string(b[5],b[6], "L")
    ##判断是不是三角形
    #elif y_A < 2 and y_A > 1.6:
        #img.draw_string(b[5],b[6], "S ")
        ## Draw a rect around the blob.

def Find_RGB_Blobs(rgb, img):
    num = 0
    blobs = img.find_blobs([rgb_threshold[rgb]],area_threshold=100,pixels_threshold = 100)
    for b in blobs:
      if(Dis_Circle(b,img)):
          num += 1
      #print(blobs)
      #迭代找到的目标颜色区域
      img.draw_rectangle(b[0:4])
    return num



def Color_Check():
  rgb_num = [0,0,0]
  rgb_list = [0,0,0]
  for i in range(20):
      sensor.set_framesize(sensor.QVGA)   # Set frame size to QVGA (320x240)
      sensor.set_windowing((90,20,180,160))
      sensor.set_contrast(3)
      sensor.set_brightness(3)
      sensor.set_saturation(3)

      img = sensor.snapshot()
      #img.mean(1)
      rgb_num[0] = Find_RGB_Blobs('r',img)
      rgb_num[1] = Find_RGB_Blobs('g',img)
      rgb_num[2] = Find_RGB_Blobs('b',img)

      if(rgb_list != rgb_num):
            rgb_list = rgb_num
      lcd.display(img)
      rgb_num = [0,0,0]
  return rgb_list


#while(True):

    #clock.tick()

    ##img = sensor.snapshot()
    ##img.bilateral(3, color_sigma=0.1, space_sigma=1)
    ##img.mean(1)
    ##for c in img.find_circles(threshold = 3800, x_margin = 10, y_margin = 10, r_margin = 10,
            ##r_min = 2, r_max = 80, r_step = 2):
        ##img.draw_circle(c.x(), c.y(), c.r(), color = (0, 0, 0))
    #print(Color_Check())




